function [AngShoulder,AngLong,AngFoot]=legAngle(i,X,Y,Z)
    %腿参数
    Lshoulder=43.9;
    Llong=150;
    Lfoot=153.7;
    AngTilt=atan(33.5/150);
    %step end
    %
    AngYZ=acos(Y/sqrt(Y*Y+Z*Z));
    if(Z<0)
        AngYZ=2*pi-AngYZ;
    end
    if (i==1||i==2)
        AngShoulder=acos(Lshoulder/sqrt(Y*Y+Z*Z))+AngYZ-2*pi;
    else
        AngShoulder=AngYZ-acos(Lshoulder/sqrt(Y*Y+Z*Z));
    end
    %计算腿部二维坐标
    Lyz=-sqrt(Y*Y+Z*Z-Lshoulder*Lshoulder);
    c3=(X*X+Lyz*Lyz-Llong*Llong-Lfoot*Lfoot)/(2*Llong*Lfoot);
    ca=(Llong+Lfoot*c3)/sqrt(Lyz*Lyz+X*X);
    Angxyz=acos(X/sqrt(Lyz*Lyz+X*X));
    if(Lyz<0)
        Angxyz=-Angxyz;
    end
    AngLong=Angxyz-acos(ca);%计算Long摆动角AngLong
    AngFoot=acos(c3)-AngTilt;%计算Foot摆动角AngFoot
end